#include "pid.h"                                                                                                                                                                                                 
#include "encoder.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "mpu6050.h"
#include "motor.h"

//传感器数据变量
int Encoder_L,Encoder_R;//编码器变量
float Pitch,Roll,Yaw;//陀螺仪变量
short gyrox,gyroy,gyroz;//角速度值
short aacx,aacy,aacz;
extern uint8_t Fore,Back,Left,Right;
extern float distance;
#define SPEED_Y 15 //俯仰(前后)最大设定速度
#define SPEED_Z 100//偏航(左右)最大设定速度

//闭环控制中间变量
int Vertical_out,Velocity_out,Turn_out;//速度环，直立环和转向环的输出
int Target_turn,Target_Speed,Target_Turn;//目标速度
float Med_Angle = -2.4 ;//平衡时角度值偏移量
int MOTO1,MOTO2;

float Vertical_Kp = 260,Vertical_Kd = 1.70;//(数量级：KP:0~1000,kd:0~10)(测得极性正确时为正)
float Velocity_Kp = -0.3,Velocity_Ki = -0.0015;//(数量级：kp:0~1)(测得极性正确时为负)
float Turn_Kp = 10,Turn_Kd = 0.6;
uint8_t stop = 0;

extern TIM_HandleTypeDef htim2;
extern TIM_HandleTypeDef htim4;

//pd直立环控制器
//输入：期望角度，真实角度，角速度
int Vertical(float Med,float Angle,float gyro_Y)
{
	int temp;
	temp = Vertical_Kp*(Angle - Med)+Vertical_Kd*gyro_Y;
	return temp;
}

//pi速度环控制器
//输入：期望速度，左编码器，右编码器
int Velocity(int Target,int encoder_L,int encoder_R)
{
	static int Err_LowOut_last,Encoder_S;
	static float a=0.7;
	int Err,Err_LowOut,temp;
	Velocity_Ki = Velocity_Kp / 200;
	//1.计算偏差值
	Err = (encoder_L + encoder_R) - Target;
	//2.低通滤波
	Err_LowOut = (1-a)*Err + a*Err_LowOut_last;
	Err_LowOut_last = Err_LowOut;
	//3.编码器积分
	Encoder_S += Err_LowOut;
	//4.积分限幅
	if(Encoder_S>20000)
		Encoder_S = 20000;
	if(Encoder_S<-20000)
		Encoder_S = -20000;
	if (stop == 1)
	{
		Encoder_S = 0;
		stop = 0;
	}
	//5.速度环计算
	temp = Velocity_Kp*Err_LowOut+Velocity_Ki*Encoder_S;
	return temp;
}


//pd转向环控制器
//输入：角速度，角度值
int Turn(float gyro_Z,int Target_turn)
{
	int temp;
	temp = Turn_Kp*Target_turn + Turn_Kd*gyro_Z;
	return temp;
}

void Control(void)//每隔10ms调用一次
{
	int PWM_out;
	//1.读取编码器和陀螺仪的数据
	Encoder_L = Read_Speed(&htim2);
	Encoder_R = -Read_Speed(&htim4);
	mpu_dmp_get_data(&Pitch,&Roll,&Yaw);
	MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);
	MPU_Get_Accelerometer(&aacx,&aacy,&aacz);
	
	//遥控
	if((Fore==0)&&(Back==0))Target_Speed=0;//未接受到前进后退指令-->速度清零，稳在原地
	if(Fore==1)
	{
		if(distance<50)
			Target_Speed--;
		else
			Target_Speed++;
	}
 
	if(Back==1){Target_Speed--;}//正负根据极性调整
	Target_Speed=Target_Speed>SPEED_Y?SPEED_Y:(Target_Speed<-SPEED_Y?(-SPEED_Y):Target_Speed);//限幅
	
	/*左右*/
	if((Left==0)&&(Right==0))Target_turn=0;
	if(Left==1)Target_turn-=30;	//左转
	if(Right==1)Target_turn+=30;	//右转
	Target_turn=Target_turn>SPEED_Z?SPEED_Z:(Target_turn<-SPEED_Z?(-SPEED_Z):Target_turn);//限幅( (20*100) * 100   )
	
	/*转向约束*/
	if((Left==0)&&(Right==0))Turn_Kd=0.6;//若无左右转向指令，则开启转向约束
	else if((Left==1)||(Right==1))Turn_Kd=0;//若左右转向指令接收到，则去掉转向约束
	
	//2.将数据传入PID控制器，计算出输出结果,即左右两个电机的输出值
	Velocity_out = Velocity(Target_Speed,Encoder_L,Encoder_R);//速度环输出
	Vertical_out = Vertical(Velocity_out + Med_Angle,Roll,gyrox);
	Turn_out = Turn(gyroz,Target_Turn);
	PWM_out = Vertical_out;
	MOTO1 = PWM_out - Turn_out;
	MOTO2 = PWM_out + Turn_out;

	//3.加载pwm数据进电机
	Limit(&MOTO1,&MOTO2);
	Load(MOTO1,MOTO2);
}








